#pragma once
#include "cybertron/sensor/noise/GPSNoiseMaker.hpp"

CYBERTRON_BEGIN

class IMUNoiseMaker : public NoiseMaker
{
public:
	IMUNoiseMaker(GPSNoiseMakerPtr pGPSNoise, float positionRandomWalk = 10.0/*meter per square hour*/, float angularRandomWalk = 0.00524/*rad per square hour*/);
	IMUNoiseMaker(const SensorGpsImuConfig& sensorConfig);
	virtual ~IMUNoiseMaker();

	void update(const vec3& pos, const vec3& rotation, uint64_t frame = 0) override;
	void syncWithGPSNoise();
	inline vec3 positionNoise() const { return m_imuPositionNoise.value3d(); }
	inline vec3 attitudeNoise() const { return m_imuAttitudeNoise.value3d(); }
	inline vec3 velocityNoise() const { return m_imuVelocityNoise.value3d(); }
	inline vec3 accelerationNoise() const { return m_imuAccelerationNoise.value3d(); }
	inline vec3 angularVelocityNoise() const { return m_imuAngularVelocityNoise.value3d(); }

protected:
	GPSNoiseMakerPtr	m_pGPSNoise;
	CompositeNoise		m_imuPositionNoise;
	CompositeNoise		m_imuAttitudeNoise;
	WhiteNoise			m_imuVelocityNoise;
	WhiteNoise			m_imuAccelerationNoise;
	WhiteNoise			m_imuAngularVelocityNoise;
	vec3				m_lastAttitude;
	float				m_lastAttitudeUpdateTime;
	std::uniform_real_distribution<float>	mGpsSignalSelector;
};

typedef std::shared_ptr<IMUNoiseMaker> IMUNoiseMakerPtr;

CYBERTRON_END